#include "userDriver.h"

PARAMTER_PID pidL = {
  2.0,    //  float Kp;
  0.0,    //  float Ki;
  0.0,    //  float Kd;

  0,      //  float ansISum;
  1000,   //  float ansIMAX;
  10,     //  float errIMAX;
  0,      //  int lastError;
  DRIVER_MAX_SPEED_PWM,      //  int target;
  0       //  float ansAll;
};

PARAMTER_PID pidR = {
  2.0,    //  float Kp;
  0.0,    //  float Ki;
  0.0,    //  float Kd;

  0,      //  float ansISum;
  1000,   //  float ansIMAX;
  10,     //  float errIMAX;
  0,      //  int lastError;
  DRIVER_MAX_SPEED_PWM,      //  int target;
  0       //  float ansAll;
};

PARAMTER_CBH cbh = {
  0.6,    //  float Kp;
  0.0,    //  float Ki;
  0.2,    //  float Kd;

  0,      //  float ansISum;
  100,    //  float ansIMAX;
  5,     //  float errIMAX;
  0,      //  int lastError;
};


/*
PARAMTER_PID pidL = {
  1.9,    //  float Kp;
  0.0,    //  float Ki;
  0.0,    //  float Kd;

  0,      //  float ansISum;
  1000,   //  float ansIMAX;
  10,     //  float errIMAX;
  0,      //  int lastError;
  DRIVER_MAX_SPEED_PWM,      //  int target;
  0       //  float ansAll;
};

PARAMTER_PID pidR = {
  1.9,    //  float Kp;
  0.0,    //  float Ki;
  0.0,    //  float Kd;

  0,      //  float ansISum;
  1000,   //  float ansIMAX;
  10,     //  float errIMAX;
  0,      //  int lastError;
  DRIVER_MAX_SPEED_PWM,      //  int target;
  0       //  float ansAll;
};

PARAMTER_CBH cbh = {
  1.1,    //  float Kp;
  0.0,    //  float Ki;
  0.3,    //  float Kd;

  0,      //  float ansISum;
  100,    //  float ansIMAX;
  5,     //  float errIMAX;
  0,      //  int lastError;
};*/


void DRIVER_PID(PARAMTER_PID* pid, int presentState, float* pwm)
{
  int error = pid->target - presentState;
  float ansP = pid->Kp * error;
  float ansI = 0;
  float ansD = 0;
  
  if(error < pid->errIMAX && error > -pid->errIMAX)
  {
    if(error == 0)
    {
      pid->ansISum = 0;
    }
    else
    {
      pid->ansISum += error * pid->Ki;
      ansI = pid->ansISum;
    }
  }
  else    pid->ansISum = 0;
  
  ansD = (float)(error - pid->lastError)*pid->Kd;
  pid->lastError = error;
  
  pid->ansAll += ansP + ansI + ansD;
  
  if(pid->ansAll > 2047) pid->ansAll = 2047;
  else if(pid->ansAll < -2047) pid->ansAll = -2047;
  
  *pwm = pid->ansAll;
}

void DRIVER_CBH(int presentState)
{
  int error = presentState;
  float ansP = cbh.Kp * error;
  float ansI = 0;
  float ansD = 0;
  float ansAll = 0;
  
  if(error < cbh.errIMAX && error > -cbh.errIMAX)
  {
    if(error == 0)
    {
      cbh.ansISum = 0;
    }
    else
    {
      cbh.ansISum += error * cbh.Ki;
      ansI = cbh.ansISum;
    }
  }
  else    cbh.ansISum = 0;
  
  ansD = (float)(error - cbh.lastError)*cbh.Kd;
  cbh.lastError = error;
  
  ansAll = ansP+ansI+ansD;
  
  if(ansAll > 1000) ansAll = 1000;
  else if(ansAll < -1000) ansAll = -1000;

  pidR.target = DRIVER_MAX_SPEED_PWM - ansAll;
  pidL.target = DRIVER_MAX_SPEED_PWM + ansAll;
  
  if(pidR.target > CBH_MAX_PWM) pidR.target = CBH_MAX_PWM;
  else if(pidR.target < -CBH_MAX_PWM) pidR.target = -CBH_MAX_PWM;
  if(pidL.target > CBH_MAX_PWM) pidL.target = CBH_MAX_PWM;
  else if(pidL.target < -CBH_MAX_PWM) pidL.target = -CBH_MAX_PWM;
}












